/**
  ******************************************************************************
  * @file 	zmotors.c 
  * @brief  Protocol header file 
  ******************************************************************************
  */
  

#include "zbPWM.h"

// global for motors
TIM_OCInitTypeDef  			TIM_OCInitStructure;
TIM_TimeBaseInitTypeDef  	TIM_TimeBaseStructure;

void initPWM1()
{
	// init all motors gpio
	initPWM1GPIO();
	// init motorA periph
	RCC_APB1PeriphClockCmd( PWMA_PB1, ENABLE );
	// init pwmA and all channels 
	setupPWM(PWMA_TIM, mygVars->PWM1FQ, &mygVars->PWM1CH1Duty, chAll, init);
}

void disablePWM1()
{
	RCC_APB1PeriphClockCmd( PWMA_PB1, DISABLE);
}

void updatePWM1(void)
{
	u8 channelsChanged = 0;
	u8 cont = false;
	
	// if there has been a change in the enabled state
	// of at least some perph
	if(mygVars->enabled != newgVars->enabled)
	{
		// if we were off 
		if(!(mygVars->enabled & enPWM1))
		{
			// and now we are on
			if(newgVars->enabled & enPWM1)
			{
				initPWM1();
				// update to know that we are on now
				mygVars->enabled = (mygVars->enabled | enPWM1);	
				cont = true;
			}
		}
		// if we were on
		if(mygVars->enabled & enPWM1)
		{
			// and now we are off
			if(!(newgVars->enabled & enPWM1))
			{
				disablePWM1();
				// clear bit so we are off next time
				mygVars->enabled = (mygVars->enabled & !enPWM1);
			}
		}
	}
	
	// if we get to this point and we are disabled just ret
	if(!(mygVars->enabled & enPWM1) && (cont == false))
	{
		return;
	}
	
	// check pwm1 and update
	if(mygVars->PWM1FQ != newgVars->PWM1FQ)
	{
		// update the frequency in the old
		mygVars->PWM1FQ = newgVars->PWM1FQ;
		setupPWM(PWMA_TIM, mygVars->PWM1FQ, &mygVars->PWM1CH1Duty, chNone, newFQ);
	}
	
	// check the duty cycles and update if needed
	if(*(u32*)(&mygVars->PWM1CH1Duty) != *(u32*)(&newgVars->PWM1CH1Duty))
	{
		if(mygVars->PWM1CH1Duty != newgVars->PWM1CH1Duty)
		{
			mygVars->PWM1CH1Duty = newgVars->PWM1CH1Duty;
			channelsChanged |= ch1;
		}
		if(mygVars->PWM1CH2Duty != newgVars->PWM1CH2Duty)
		{
			mygVars->PWM1CH2Duty = newgVars->PWM1CH2Duty;
			channelsChanged |= ch2;
		}
		if(mygVars->PWM1CH3Duty != newgVars->PWM1CH3Duty)
		{
			mygVars->PWM1CH3Duty = newgVars->PWM1CH3Duty;
			channelsChanged |= ch3;
		}
		if(mygVars->PWM1CH4Duty != newgVars->PWM1CH4Duty)
		{
			mygVars->PWM1CH4Duty = newgVars->PWM1CH4Duty;
			channelsChanged |= ch4;
		}
		// send it to the unit, only changing changed pulse widths
		setupPWM(PWMA_TIM, mygVars->PWM1FQ, &mygVars->PWM1CH1Duty, channelsChanged, newDuty);
	}
}


void initPWM1GPIO(void)
{
	GPIO_InitTypeDef  		GPIO_InitStructure;

	// enable periph, for gpio motor2
	RCC_APB2PeriphClockCmd( PWMA_PAB2 , ENABLE);	
	// set it up and init
	GPIO_InitStructure.GPIO_Pin 		= PWMA_S0 | PWMA_S1;
	GPIO_InitStructure.GPIO_Mode 		= GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed 		= GPIO_Speed_50MHz;
	GPIO_Init(PWMA_PORT1, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin 		= PWMA_S2 | PWMA_S3;
	GPIO_InitStructure.GPIO_Mode 		= GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed 		= GPIO_Speed_50MHz;
	GPIO_Init(PWMA_PORT2, &GPIO_InitStructure);

	GPIO_ResetBits(PWMA_PORT1, PWMA_S0);
	GPIO_ResetBits(PWMA_PORT1, PWMA_S1);
	GPIO_ResetBits(PWMA_PORT1, PWMA_S2);
	GPIO_ResetBits(PWMA_PORT1, PWMA_S3);
}

/**
	this will setup all parts of the pwm

*/
void setupPWM(TIM_TypeDef * timer, float frequency, u8 duty[4], u8 channels, u8 newState)
{
	u16 prescalar			= 0;
	u8 i					= 0;
	u16 period				= 0;
	u16 pulse				= 0;
	u8	lDuty				= 0;
	// function pointer to init the channel
	void (*chanInit)(TIM_TypeDef*, TIM_OCInitTypeDef*);	 	

	// sanity check
	if(frequency > 1000000 || frequency < 1)
	{
		frequency = 10000;
	}
    
	// we use period for duty cycle too 
	period = frequencyToPeriod(frequency, &prescalar);

	if(newState == init || newState == newFQ)
	{

		// setup our frequency
		
		TIM_Cmd( timer, DISABLE ); 	// in case they are restarting
	
		// Configure timebase, Freq = clk/(period+1) = 72/12 = 6MHz
		TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
		TIM_TimeBaseStructure.TIM_Period			= period;				// active auto reload register
		TIM_TimeBaseStructure.TIM_Prescaler			= prescalar; 			// set this to 2 and its half the speed ? 
		TIM_TimeBaseStructure.TIM_ClockDivision		= 0;
		TIM_TimeBaseStructure.TIM_CounterMode		= TIM_CounterMode_Up;
		TIM_TimeBaseStructure.TIM_RepetitionCounter	= 0;
		TIM_TimeBaseInit( timer, &TIM_TimeBaseStructure );

		//Enable the timer (starts generating pulses)
		TIM_Cmd(timer, ENABLE);
	}
	
	// for each channel see if it should be enabled/updated
	for(i=0; i<=4; i++)
	{
		if(channels & 1<<i)
		{
			// pick init function to call for channel
			switch(i)
			{
			case 0:
				chanInit = &TIM_OC1Init;
				break;
			case 1:
				chanInit = &TIM_OC2Init;
				break;
			case 2:
				chanInit = &TIM_OC3Init;
				break;
			case 3:
				chanInit = &TIM_OC4Init;
				break;
			default:
				continue;
			}
		    
			// setup the pulse width
			lDuty = duty[i];
			pulse = dutyCycleToPulse(lDuty, period);

			// set the duty and 
			TIM_OCInitStructure.TIM_OCMode				= TIM_OCMode_PWM1;
			TIM_OCInitStructure.TIM_OutputState			= TIM_OutputState_Enable;
			TIM_OCInitStructure.TIM_OutputNState		= TIM_OutputNState_Disable;
			TIM_OCInitStructure.TIM_Pulse				= pulse;
			TIM_OCInitStructure.TIM_OCPolarity			= TIM_OCPolarity_High;
			(*chanInit)(PWMA_TIM, &TIM_OCInitStructure);
		}
	}
	
	//Enable the OC pwm output
	TIM_CtrlPWMOutputs(timer, ENABLE);
}





// UTILITIY AND CONVERSION FUNCTIONS 

u16 frequencyToPeriod(float frequency, u16 *prescalar)
{
	u16 orgfrequency = frequency; 
	u16	period		 = (u16)((SystemCoreClock/frequency) -1);	
	prescalar		 = 0;
	
	// if they are using a frequency low enough to merit a prescalar
	//period = (SystemCoreClock / fq ) - 1 ... ex 11 = 6mhz
	while(frequency < 1098)
	{
		prescalar++;
		frequency 	+= orgfrequency;
		period		= (u16)((SystemCoreClock/frequency) -1); 
	}
	return period;
}

// for now dutyC must be 0-100 which is 
u16 dutyCycleToPulse(u8 duty, u16 period)
{
	u8 percent = duty/10;

	if(duty > 100)
		duty = 100;


	return (percent*(period-1))/10; 
}


/*
void updateMotorA(void)
{

	u16 newPulseA0 = 0;
	u16 newPulseA1 = 0;
	u8 motorSpeed = gVars->motorAspeed;

	// if it has not changed stop here ! 
	if(motorSpeed == OldPWMASpeed)
	{
		return;
	}
	OldPWMASpeed = motorSpeed;
	 
	// check if its a negative number, that means we are going backwards
	if(motorSpeed > 127)
	{
		motorSpeed = (motorSpeed *- 1); // strip off the sign bit
		if(motorSpeed>100) motorSpeed = 100; // we only go to 100%
	  	newPulseA1 = 0;
	  	newPulseA0 = dutyCycleToPulse(motorSpeed, period);
	}else{
		if(motorSpeed>100) motorSpeed = 100;
	  	newPulseA1 = dutyCycleToPulse(motorSpeed, period);
	  	newPulseA0 = 0;
	}

	// if nothing has changed return
	if(newPulseA0 == pulseA0 && newPulseA1 == pulseA1)
	{
		return; 
	}else{
		pulseA0 = newPulseA0;
		pulseA1 = newPulseA1;
	}


	// Configure pwm output select 0
	TIM_OCInitStructure.TIM_OCMode				= TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState			= TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_OutputNState		= TIM_OutputNState_Disable;
	TIM_OCInitStructure.TIM_Pulse				= pulseA0;
	TIM_OCInitStructure.TIM_OCPolarity			= TIM_OCPolarity_High;
	PWMA_INIT_S0 // define handles what channel we output on

	// Configure pwm output	select 1
	TIM_OCInitStructure.TIM_OCMode				= TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState			= TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_OutputNState		= TIM_OutputNState_Disable;
	TIM_OCInitStructure.TIM_Pulse				= pulseA1;
	TIM_OCInitStructure.TIM_OCPolarity			= TIM_OCPolarity_High;
	PWMA_INIT_S1 // define handles what channel we output on
	
}

void setMotorPWMFreqency(TIM_TypeDef * TIM, float frequency, u8 duty)
{

	TIM_TimeBaseInitTypeDef  	TIM_TimeBaseStructure;

	u8 prescalar			= 0;
	period					= frequencyToPeriod(frequency, &prescalar);	
	
	TIM_Cmd( TIM, DISABLE );

	//GPIO_PinRemapConfig(GPIO_PartialRemap2_TIM2, ENABLE);

	// Configure timebase, Freq = clk/(period+1) = 72/12 = 6MHz
	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.TIM_Period			= period;				// active auto reload register
	TIM_TimeBaseStructure.TIM_Prescaler			= prescalar; 			// set this to 2 and its half the speed ? 
	TIM_TimeBaseStructure.TIM_ClockDivision		= 0;
	TIM_TimeBaseStructure.TIM_CounterMode		= TIM_CounterMode_Up;
	TIM_TimeBaseStructure.TIM_RepetitionCounter	= 0;
	TIM_TimeBaseInit( TIM, &TIM_TimeBaseStructure );
	
}
*/

